// -----------------------------------------------------------------------------
// This file was autogenerated by symforce from template:
//     backends/cpp/templates/function/FUNCTION.h.jinja
// Do NOT modify by hand.
// -----------------------------------------------------------------------------

#pragma once

#include <matrix/math.hpp>

namespace sym {

/**
 * This function was autogenerated from a symbolic function. Do not modify by hand.
 *
 * Symbolic function: compute_airspeed_innov_and_innov_var
 *
 * Args:
 *     state: Matrix24_1
 *     P: Matrix24_24
 *     airspeed: Scalar
 *     R: Scalar
 *     epsilon: Scalar
 *
 * Outputs:
 *     innov: Scalar
 *     innov_var: Scalar
 */
template <typename Scalar>
void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
                                     const matrix::Matrix<Scalar, 24, 24>& P, const Scalar airspeed,
                                     const Scalar R, const Scalar epsilon,
                                     Scalar* const innov = nullptr,
                                     Scalar* const innov_var = nullptr) {
  // Total ops: 69

  // Input arrays

  // Intermediate terms (7)
  const Scalar _tmp0 = -state(23, 0) + state(5, 0);
  const Scalar _tmp1 = -state(22, 0) + state(4, 0);
  const Scalar _tmp2 = std::sqrt(Scalar(std::pow(_tmp0, Scalar(2)) + std::pow(_tmp1, Scalar(2)) +
                                        epsilon + std::pow(state(6, 0), Scalar(2))));
  const Scalar _tmp3 = Scalar(1.0) / (_tmp2);
  const Scalar _tmp4 = _tmp3 * state(6, 0);
  const Scalar _tmp5 = _tmp1 * _tmp3;
  const Scalar _tmp6 = _tmp0 * _tmp3;

  // Output terms (2)
  if (innov != nullptr) {
    Scalar& _innov = (*innov);

    _innov = _tmp2 - airspeed;
  }

  if (innov_var != nullptr) {
    Scalar& _innov_var = (*innov_var);

    _innov_var = R +
                 _tmp4 * (-P(22, 6) * _tmp5 - P(23, 6) * _tmp6 + P(4, 6) * _tmp5 + P(5, 6) * _tmp6 +
                          P(6, 6) * _tmp4) -
                 _tmp5 * (-P(22, 22) * _tmp5 - P(23, 22) * _tmp6 + P(4, 22) * _tmp5 +
                          P(5, 22) * _tmp6 + P(6, 22) * _tmp4) +
                 _tmp5 * (-P(22, 4) * _tmp5 - P(23, 4) * _tmp6 + P(4, 4) * _tmp5 + P(5, 4) * _tmp6 +
                          P(6, 4) * _tmp4) -
                 _tmp6 * (-P(22, 23) * _tmp5 - P(23, 23) * _tmp6 + P(4, 23) * _tmp5 +
                          P(5, 23) * _tmp6 + P(6, 23) * _tmp4) +
                 _tmp6 * (-P(22, 5) * _tmp5 - P(23, 5) * _tmp6 + P(4, 5) * _tmp5 + P(5, 5) * _tmp6 +
                          P(6, 5) * _tmp4);
  }
}  // NOLINT(readability/fn_size)

// NOLINTNEXTLINE(readability/fn_size)
}  // namespace sym
